/*
 * Copyright The Slam Authors
 */

#ifndef SLAM_MAPPING_REFLECTOR_DETECTOR_H_
#define SLAM_MAPPING_REFLECTOR_DETECTOR_H_

#include <ceres/ceres.h>
#include <deque>
#include <random>

#include "slam/mapping/internal/2d/line_extraction.h"
#include "slam/mapping/internal/optimization/cost_functions/delta_pose_cost_function_2d.h"
#include "slam/mapping/pose_graph_interface.h"
#include "slam/mapping/proto/reflector_dector_options.pb.h"
#include "slam/sensor/landmark_data.h"
#include "slam/sensor/point_cloud.h"
#include "slam/transform/rigid_transform.h"

namespace slam {
namespace mapping {

// 反光板类型
enum ReflectorType {
  // 板
  REFLECTOR_PLANE = 0,
  // 柱
  REFLECTOR_COLUMN = 1,
};

// 反光柱拟合方法
enum CircleFitMethod {
  NONE = 0,
  RANSAC = 1,
  LEAST_SQUARE = 2,
};

struct LandmarkObservation {
  int node_id;
  int landmark_id;
  Eigen::Vector2d p_tracking_landmark;
};

struct Landmark {
  int landmark_id = -1;
  int num_node_not_observed_ = 0;
  std::vector<LandmarkObservation> observations;
  Eigen::Vector2d p_local_landmark;
};

struct LocalNode {
  int node_id;
  transform::Rigid2d T_local_tracking;
  std::vector<LandmarkObservation> observations;
};

typedef std::shared_ptr<LocalNode> LocalNodePtr;
typedef std::shared_ptr<Landmark> LandmarkPtr;

void ComputeReflectorCoordinateTransform(
    const std::vector<sensor::MatchedReflectorInfo>& matched_reflector_info,
    const transform::Rigid2d& init_pose, transform::Rigid2d* matched_pose);

class DistanceFromCircleCost {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(const double xx,
                                                         const double yy) {
    return new ceres::AutoDiffCostFunction<
        DistanceFromCircleCost, 1 /* residuals */, 3 /* variables */>(
        new DistanceFromCircleCost(xx, yy));
  }

  // r = m^2
  template <typename T>
  bool operator()(const T* const xym, T* residual) const {
    T r = xym[2] * xym[2];
    T xp = xx_ - xym[0];
    T yp = yy_ - xym[1];

    residual[0] = r * r - xp * xp - yp * yp;
    return true;
  }

 private:
  explicit DistanceFromCircleCost(const double xx, const double yy)
      : xx_(xx), yy_(yy) {}

  DistanceFromCircleCost(const DistanceFromCircleCost&) = delete;
  DistanceFromCircleCost& operator=(const DistanceFromCircleCost&) = delete;

  const double xx_;
  const double yy_;
};

class CoordinateTransformCost2D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const Eigen::Vector3d& pose_1, const Eigen::Vector3d& pose_2,
      const double weight) {
    return new ceres::AutoDiffCostFunction<
        CoordinateTransformCost2D, 2 /* residuals */, 3 /* pose variables */>(
        new CoordinateTransformCost2D(pose_1, pose_2, weight));
  }

  template <typename T>
  bool operator()(const T* const x_y_yaw, T* residual) const {
    T x_new = T(pose_1_[0]) * cos(x_y_yaw[2]) -
              T(pose_1_[1]) * sin(x_y_yaw[2]) + x_y_yaw[0];
    T y_new = T(pose_1_[0]) * sin(x_y_yaw[2]) +
              T(pose_1_[1]) * cos(x_y_yaw[2]) + x_y_yaw[1];

    residual[0] = weight_ * (x_new - T(pose_2_[0]));
    residual[1] = weight_ * (y_new - T(pose_2_[1]));
    return true;
  }

 private:
  explicit CoordinateTransformCost2D(const Eigen::Vector3d& pose_1,
                                     const Eigen::Vector3d& pose_2,
                                     const double weight)
      : pose_1_(pose_1), pose_2_(pose_2), weight_(weight) {}

  CoordinateTransformCost2D(const CoordinateTransformCost2D&) = delete;
  CoordinateTransformCost2D& operator=(const CoordinateTransformCost2D&) =
      delete;

  const Eigen::Vector3d pose_1_;
  const Eigen::Vector3d pose_2_;
  const double weight_;
};

class ReflectorDetect {
 public:
  ReflectorDetect(const proto::ReflectorDectorOptions& options);
  ReflectorDetect() = delete;
  ~ReflectorDetect() {}

  void ClusterPoints(const sensor::PointCloud& points,
                     sensor::PointCloud* centers_ptr,
                     std::vector<int>* count_ptr);

  void ExtractReflectorFeature(const sensor::PointCloud& reflector_raw_data,
                               sensor::PointCloud* reflector_data_ptr);

  void ReflectorMatching(
      const std::map<int, LandmarkPtr>& reflectors_map,
      const transform::Rigid2d& T_local_tracking,
      const sensor::PointCloud& reflector_feature_in_tracking,
      std::vector<int>* match_reflector_ids_ptr,
      std::vector<double>* match_min_dists_ptr);

  void ReflectorMatching(
      const std::map<std::string, Eigen::Vector3d>& reflectors_nearby,
      const transform::Rigid3d& T_global_tracking,
      const sensor::PointCloud& reflector_feature_in_tracking,
      std::vector<std::string>* match_reflector_ids_ptr,
      std::vector<double>* match_min_dists_ptr);

 private:
  void ExtractPlaneFeature(const sensor::PointCloud& points,
                           sensor::PointCloud* centers_ptr,
                           std::vector<int>* count_ptr);

  void ExtractColumnFeature(const sensor::PointCloud& points,
                            sensor::PointCloud* centers_ptr,
                            std::vector<int>* count_ptr);

  void RansacCircle(const std::vector<sensor::PointCloud>& cluster_points,
                    sensor::PointCloud* centers_ptr,
                    std::vector<int>* count_ptr);

  void LeastSquareCircle(const std::vector<sensor::PointCloud>& cluster_points,
                         sensor::PointCloud* centers_ptr,
                         std::vector<int>* count_ptr);

 private:
  const proto::ReflectorDectorOptions options_;
  std::unique_ptr<LineExtraction> line_extraction_;
};

}  // namespace mapping
}  // namespace slam

#endif  // SLAM_MAPPING_DETECT_FLOORS_H_
